
#include "My_Inc/PHY/PHY_TOGI.h"
/*==================================TOGI初始化===================================*/
/*
 * 函数名：TOGI_Init
 * 功能：初始化TOGI变换
 * 参数：
 * 		transform* transform：变换结构体
 * 		float TOGI_k：TOGI增益K
 * 		float TOGI_T：TOGI时间常数T_ADC
 * 返回值：无
 */
static inline void TOGI_Reset_Inline(TOGI_Typedef *transform);
void               TOGI_Init(TOGI_Typedef *transform, float TOGI_k, float TOGI_T)
{
    if (transform == NULL)
        return;
    transform->TOGI_k = TOGI_k;        // TOGI增益
    transform->TOGI_T = TOGI_T * 0.5f; // TOGI时间常数
    TOGI_Reset_Inline(transform);      // 重置TOGI变换结构体
}
void TOGI_Reset(TOGI_Typedef *transform)
{
    if (transform == NULL)
        return;
    TOGI_Reset_Inline(transform); // 重置TOGI变换结构体
}
static inline void TOGI_Reset_Inline(TOGI_Typedef *transform)
{

    transform->integral1_Yn = 0.0f;   // 积分器1
    transform->integral1_Xn = 0.0f;   // 积分器1
    transform->integral1_Xn_1 = 0.0f; // 积分器1

    transform->integral2_Yn = 0.0f;   // 积分器2
    transform->integral2_Xn = 0.0f;   // 积分器2
    transform->integral2_Xn_1 = 0.0f; // 积分器2

    transform->integral3_Yn = 0.0f;   // 积分器3
    transform->integral3_Xn = 0.0f;   // 积分器3
    transform->integral3_Xn_1 = 0.0f; // 积分器3

    transform->U1 = 0.0f; // 输出1
    transform->U2 = 0.0f; // 输出2
    transform->U3 = 0.0f; // 输出3

    transform->Alpha = 0.0f; // Alpha电压
    transform->Beta = 0.0f;  // Beta电压
    transform->D = 0.0f;     // D电压
    transform->Q = 0.0f;     // Q电压
}
/*==================================TOGI变换===================================*/
/*
 * 函数名：TOGI_to_Alpha_Beta_Zero
 * 功能：实现TOGI变换到Alpha_Beta_Zero
 * 参数：
 * 		TOGI_Typedef* transform：变换结构体
 * 		float Input：输入值
 * 		float W：权重
 * 返回值：无
 */
void TOGI_to_Alpha_Beta_Zero(TOGI_Typedef *transform, float Input, float W)
{
    float temp = (Input - transform->U1) * transform->TOGI_k;
    transform->integral1_Xn = (temp - transform->U3) * W;
    transform->integral1_Yn += transform->TOGI_T * (transform->integral1_Xn + transform->integral1_Xn_1); // 积分
    transform->integral1_Xn_1 = transform->integral1_Xn;                                                  // 移位
    transform->U3 = transform->integral1_Yn;

    transform->integral2_Xn = (temp - transform->U2) * W;
    transform->integral2_Yn += +transform->TOGI_T * (transform->integral2_Xn + transform->integral2_Xn_1); // 积分
    transform->integral2_Xn_1 = transform->integral2_Xn;                                                   // 移位
    transform->U1 = transform->integral2_Yn;

    transform->integral3_Xn = (transform->U1 * W);
    transform->integral3_Yn += transform->TOGI_T * (transform->integral3_Xn + transform->integral3_Xn_1); // 积分
    transform->integral3_Xn_1 = transform->integral3_Xn;                                                  // 移位
    transform->U2 = transform->integral3_Yn;

    transform->Alpha = transform->U1;
    transform->Beta = transform->U2 - transform->U3;
}

/*==================================TOGI变换到DQ===================================*/
/*
 * 函数名：TOGI_to_DQ0
 * 功能：实现TOGI变换到DQ0
 * 参数：
 * 		TOGI_Typedef* transform：变换结构体
 * 		float Input：输入值
 * 		float W：权重
 * 		float* Sin_Data：正弦数据指针
 * 		float* Cos_Data：余弦数据指针
 * 返回值：无
 */
void TOGI_to_DQ0(TOGI_Typedef *transform, float Input, float W, float *Sin_Data, float *Cos_Data)
{
    TOGI_to_Alpha_Beta_Zero(transform, Input, W);
    // DQ0变换
    transform->D = transform->Alpha * (*Cos_Data) + transform->Beta * (*Sin_Data);
    transform->Q = -transform->Alpha * (*Sin_Data) + transform->Beta * (*Cos_Data);
}

/*==================================DQ到TOGI变换===================================*/
/*
 * 函数名：DQ0_to_TOGI
 * 功能：实现DQ0到TOGI变换
 * 参数：
 * 		TOGI_Typedef* transform：变换结构体
 * 		float* Sin_Data：正弦数据指针
 * 		float* Cos_Data：余弦数据指针
 * 返回值：无
 */
void DQ0_to_TOGI(TOGI_Typedef *transform, float *Sin_Data, float *Cos_Data)
{
    transform->Alpha = transform->D * (*Cos_Data) - transform->Q * (*Sin_Data);
}
